#include <stdarg.h>
#include <stdio.h>
#include <stdint.h>

#include "drivers/spi.h"
#include "drivers/cc2420.h"
#include "common.h"
#include "error.h"

#define FIFOP		2
#define FIFO		3
#define SFD			1

#define CC2420_RECV_BUFFER_SIZE 16

static void cc2420_flush_rx(void);

static enum cc2420_state state;

static struct {
	uint8_t data[CC2420_RECV_BUFFER_SIZE];
	// The index of the next free location
	uint16_t head;
	// The index of the last written char
	uint16_t tail;
} rx_buffer;

/*static void read_rx_buffer(void)
{
	uart_write_n(rx_buffer.data, CC2420_RECV_BUFFER_SIZE);
}*/

/* Gets the remaining FREE bytes in the buffer */
static inline uint16_t rx_buf_get_free_bytes(void)
{
	uint16_t free = 0;

	if (rx_buffer.head >= rx_buffer.tail)
	{
		free = CC2420_RECV_BUFFER_SIZE - (rx_buffer.head - rx_buffer.tail);
	}
	else
	{
		free = rx_buffer.tail - rx_buffer.head;
	}
	//print_buffer();

	//uart_write_n(&free, 2);

	return free;
}

void print_buffer()
{
	printf("h: %d t: %d\n", rx_buffer.head, rx_buffer.tail);
	for (uint16_t i = 0; i < CC2420_RECV_BUFFER_SIZE; i++)
	{
		printf("%x(%c), ", rx_buffer.data[i], rx_buffer.data[i]);
	}
	printf("\n");
}

void cc2420_read_rxfifo()
{
	//while (P1IN & BV(FIFOP))
	{
		spi_send_receive(SREG | SREAD | CC2420_RXFIFO);
		uint8_t len = spi_send_receive(SREG | SREAD | CC2420_SNOP);

		if (len > 127)
		{
			//cc2420_flush_rx();
			printf("flush\n");
		}
		else if (rx_buf_get_free_bytes() > (len + 1))
		{
			rx_buffer.data[rx_buffer.head] = len;
			rx_buffer.head = (rx_buffer.head + 1) % CC2420_RECV_BUFFER_SIZE;

			for (uint16_t i = 0; i < len; i++)
			{
				rx_buffer.data[rx_buffer.head] = \
					spi_send_receive(SREG | SREAD | CC2420_SNOP);
				rx_buffer.head = (rx_buffer.head + 1) % CC2420_RECV_BUFFER_SIZE;
			}
		}
		else
		{
			printf("no Space in rx ring buffer\n");
		}
	}
	//__delay_cycles(0.00002*SYSFREQ);
	//cc2420_flush_rx();
}

uint8_t cc2420_get_frame(uint8_t * b)
{
	uint16_t len = 0;
	if (rx_buf_get_free_bytes() != CC2420_RECV_BUFFER_SIZE)
	{
		len = rx_buffer.data[rx_buffer.tail];
		rx_buffer.tail = (rx_buffer.tail + 1) % CC2420_RECV_BUFFER_SIZE;

		/* Skip the checksum */
		//len -= 2;
		b[0] = len;

		for (uint16_t i = 1; i < (len + 1); i++)
		{
			b[i] = rx_buffer.data[rx_buffer.tail];
			rx_buffer.tail = (rx_buffer.tail + 1) % CC2420_RECV_BUFFER_SIZE;
		}
	}
	return len;
}

void cc2420_write_txfifo(uint8_t len, uint8_t * psdu)
{
	cc2420_strobe(CC2420_SFLUSHTX);

	spi_send_receive(SREG | SWRITE | CC2420_TXFIFO);
	spi_send_receive(len);

	for (uint16_t i = 0; i < len; i++)
	{
		spi_send_receive(psdu[i]);
	}
}
